#include "main.h"

// 检测状态

#define LOCK_MODE_IDLE 0x00
#define LOCK_MODE_START_R 0x01
#define LOCK_MODE_START_L 0x02
#define LOCK_MODE_RUNING 0x04
#define LOCK_MODE_START_AUTO 0x10
#define LOCK_MODE_AUTO_UNLOCKED_ING 0x14
#define LOCK_MODE_AUTO_UNLOCKED_END 0x15
#define LOCK_MODE_AUTO_START_LOCKED 0x16
#define LOCK_MODE_AUTO_LOCKED_ING 0x17
#define LOCK_MODE_AUTO_LOCKED_END 0x18

#define LOCK_CTL_STOP 0x00
#define LOCK_CTL_R 0x01
#define LOCK_CTL_L 0x02
#define LOCK_CTL_AUTO 0x03

static uint8_t lock_det_o = 0;
static uint8_t lock_det_s = 0;
static uint8_t lock_det_k = 0;

static uint8_t lock_id[3] = {0};

static uint8_t lock_mode = LOCK_MODE_IDLE;
static uint8_t lock_control = LOCK_CTL_STOP;

static uint16_t lock_time = 0;

void lock_id_init(void);
void check_id();
void response_lock_id();

void cmd_lock_handle(uint8_t cmd[], uint8_t len)
{
    // if(len < 0)
    //     return;
    // 处理开锁
    if (cmd[0] == 0x10)
    {
        if (cmd[1] == 0x01)
        {
            lock_control = LOCK_CTL_R;
        }
        else if (cmd[1] == 0x02)
        {
            lock_control = LOCK_CTL_L;
        }
        else if (cmd[1] == 0x03)
        {
            lock_control = LOCK_CTL_AUTO;
        }
        else
        {
            lock_control = LOCK_CTL_STOP;
        }
        // response_ask(1);
    }
    else if (cmd[0] == 0x20) // 查询状态
    {
        response_lock_status();
    }
    else if (cmd[0] == 0x21) // 查询ID
    {
        response_lock_id();
    }
}

//
void lock_init(void)
{
    // det_o init
    GPIO_Init(LOCK_DET_O_PORT, (GPIO_Pin_TypeDef)LOCK_DET_O_PIN, GPIO_MODE_IN_FL_NO_IT);
    // det_s init
    GPIO_Init(LOCK_DET_S_PORT, (GPIO_Pin_TypeDef)LOCK_DET_S_PIN, GPIO_MODE_IN_FL_NO_IT);
    // det_k init
    GPIO_Init(LOCK_DET_K_PORT, (GPIO_Pin_TypeDef)LOCK_DET_K_PIN, GPIO_MODE_IN_FL_NO_IT);
    lock_id_init();
}

//
void lock_id_init(void)
{
    GPIO_Init(LOCK_ID_1_PORT, (GPIO_Pin_TypeDef)LOCK_ID_1_PIN, GPIO_MODE_OUT_PP_LOW_FAST);
    GPIO_Init(LOCK_ID_2_PORT, (GPIO_Pin_TypeDef)LOCK_ID_2_PIN, GPIO_MODE_OUT_PP_LOW_FAST);
    GPIO_Init(LOCK_ID_3_PORT, (GPIO_Pin_TypeDef)LOCK_ID_3_PIN, GPIO_MODE_OUT_PP_LOW_FAST);
}

void check_id(void)
{
    if (GPIO_ReadInputPin(LOCK_ID_1_PORT, LOCK_ID_1_PIN))
    {
        lock_id[0] = 1;
    }
    else
    {
        lock_id[0] = 0;
    }
    if (GPIO_ReadInputPin(LOCK_ID_2_PORT, LOCK_ID_2_PIN))
    {
        lock_id[1] = 1;
    }
    else
    {
        lock_id[1] = 0;
    }
    if (GPIO_ReadInputPin(LOCK_ID_3_PORT, LOCK_ID_3_PIN))
    {
        lock_id[2] = 1;
    }
    else
    {
        lock_id[2] = 0;
    }
}

// 检测锁开关状态
uint8_t check_lock_switch(void)
{
    if (GPIO_ReadInputPin(LOCK_DET_O_PORT, LOCK_DET_O_PIN))
    {
        lock_det_o = 1;
    }
    else
    {
        lock_det_o = 0;
    }
    if (GPIO_ReadInputPin(LOCK_DET_S_PORT, LOCK_DET_S_PIN))
    {
        lock_det_s = 1;
    }
    else
    {
        lock_det_s = 0;
    }
    if (GPIO_ReadInputPin(LOCK_DET_K_PORT, LOCK_DET_K_PIN))
    {
        lock_det_k = 1;
    }
    else
    {
        lock_det_k = 0;
    }
    check_id();

    return 0;
}

// 控制锁电机
void control_lock_motor(void)
{
    switch (lock_mode)
    {
    case LOCK_MODE_IDLE:
        if (motor_status() != 0x00)
            motor_run(0);
        if (lock_control == LOCK_CTL_L)
        {
            lock_mode = LOCK_MODE_START_L;
        }
        else if (lock_control == LOCK_CTL_R)
        {
            lock_mode = LOCK_MODE_START_R;
            lock_time = 0;
        }
        else if (lock_control == LOCK_CTL_AUTO)
        {
            lock_mode = LOCK_MODE_START_AUTO;
            lock_time = 0;
        }
        break;
    case LOCK_MODE_START_L:
        motor_run(1);
        lock_mode = LOCK_MODE_RUNING;
        lock_time = 0;
        break;
    case LOCK_MODE_START_R:
        motor_run(2);
        lock_mode = LOCK_MODE_RUNING;
        lock_time = 0;
        break;
    case LOCK_MODE_RUNING:
        if (motor_status() != 0x00)
        {
            if (lock_time > 20)
            {
                lock_control = LOCK_CTL_STOP;
                lock_mode = LOCK_MODE_IDLE;
            }
            else
            {
                lock_time++;
                SysTimer_DelayMs(10);
            }
        }
        else
        {

            lock_control = LOCK_CTL_STOP;
            lock_mode = LOCK_MODE_IDLE;
        }
        break;
    case LOCK_MODE_START_AUTO:
        motor_run(2);
        lock_mode = LOCK_MODE_AUTO_UNLOCKED_ING;
        lock_time = 0;
        break;
    case LOCK_MODE_AUTO_UNLOCKED_ING:
        if (motor_status() != 0x00)
        {
            if (lock_time > 20)
            {
                lock_control = LOCK_CTL_STOP;
                lock_mode = LOCK_MODE_AUTO_UNLOCKED_END;
                lock_time = 0;
            }
            else
            {
                lock_time++;
                SysTimer_DelayMs(10);
            }
        }
        else
        {

            lock_control = LOCK_CTL_STOP;
            lock_mode = LOCK_MODE_AUTO_UNLOCKED_END;
            lock_time = 0;
        }
        break;
    case LOCK_MODE_AUTO_UNLOCKED_END:
        if(lock_time == 0)
        {
            motor_run(0);
        }
        else if (lock_time > 500)
        {
            lock_mode = LOCK_MODE_AUTO_START_LOCKED;
            lock_time = 0; 
        }
        lock_time++;
        SysTimer_DelayMs(10);
        break;
    case LOCK_MODE_AUTO_START_LOCKED:
        motor_run(1);
        lock_mode = LOCK_MODE_AUTO_LOCKED_ING;
        lock_time = 0;
        break;
    case LOCK_MODE_AUTO_LOCKED_ING:
        if (motor_status() != 0x00)
        {
            if (lock_time > 20)
            {
                lock_control = LOCK_CTL_STOP;
                lock_mode = LOCK_MODE_AUTO_LOCKED_END;
                lock_time = 0;
            }
            else
            {
                lock_time++;
                SysTimer_DelayMs(10);
            }
        }
        else
        {

            lock_control = LOCK_CTL_STOP;
            lock_mode = LOCK_MODE_AUTO_LOCKED_END;
            lock_time = 0;
        }
        break;
    case LOCK_MODE_AUTO_LOCKED_END:
        lock_mode = LOCK_MODE_IDLE;
        break;
    default:
        break;
    }
}

void lock_handle_pro(void)
{
    // 检测det_o
    check_lock_switch();
    control_lock_motor();
}

/**
 * 反馈锁的状态
 */
void response_lock_status(void)
{
    uint8_t cmd[] = {0xF5, 0x5F, 0x04, 0xEE, 0x00, 0x00, 0x00};
    uint8_t i = 0;

    cmd[4] = lock_det_o;
    cmd[5] = lock_det_s;
    cmd[6] = lock_det_k;

    for (i = 0; i < 7; i++)
    {
        send_router(cmd[i]);
    }
}

/**
 * 反馈锁id
 */
void response_lock_id(void)
{
    uint8_t cmd[] = {0xF5, 0x5F, 0x02, 0xEF, 0x00};
    uint8_t i = 0;

    cmd[4] = lock_id[2] << 2 | lock_id[1] << 1 | lock_id[0];

    for (i = 0; i < 5; i++)
    {
        send_router(cmd[i]);
    }
}

/**
 * 处理应答
 */

void response_ask(uint8_t ask)
{
    uint8_t ask_cmd[] = {0xF5, 0x5F, 0x02, 0xAA, 0x00};
    ask_cmd[4] = ask;

    send_485_str(ask_cmd, sizeof(ask_cmd));
}